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SUMMARY 


This progress report summarizes the 
research work performed at The Catholic 
University Of America on the research 
grant entitled “Active Control of Robot 
Manipulator Compliance? " (Grant#: NAG 
5-780) ? supported by NASA/Goddard space 
Flight Center dur inq the period of May 
15th? 1986 to November 15th? 1986- 

In this report we first present 
the model 1 ing of the two-degree-of- 
freedom robot. Then the complete system 
including the robot and the hybrid 
controller is simulated on an IBM- XT 
Personal Computer. Simulation results 
showed that proper adjustments of 
controller n a i ns enab 1 e the rob c» t t o 
perform successful operations. Further 
research should focus on developing a 
guideline for the controller gain design 
to achieve system stability. 
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TABLE OF SYMBOLS 


Values are given in SI Units. 

A vector from origin of compliant platform to 

i 

lower attachment points of actuator i with 
respect to the fixed coordinate system 

A known vector in moving coordinate system from 

i,m 

origin of moving coordinate system to lower 
attachment point of actuator i 

B known vector in fixed coordinate system from 

i 

origin of fiiied coordinate system to upper 
attachment point of actuator i 

d known distance between the upper attachment 

points of actuators 1 and E 

F force 

f ( ) vect o r f unc t i o n 

F vector of force error in Cartesian variables 

e 


v 



F 

d 


f 

m 


vector of desired force 


actuating forces 


f reaction force 

y 
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acceleration of gravity 


I 


i ner t i a moment 


[13 


I dent i ty matr i >: 


J 


Jacob i an 


K stiffness of the reaction surface 

£ 


K 

r P 


rf 


proportional controller gains for position and 
force? respectively 


derivative controller gain 

K vector in fixed coordinate system from upper 

attachment point to lower attachment point of 
actuator i 


j 1^ j actual magnitude of actuator i obtained by 

a 

measurement 


vi 



L 


Lagrangian 


fit , M mass 

P potential energy 

q generalized coordinates 

i 

Q generalized forces 

i 

q vector of position error in joint variables 

e 

F: vector in fixed coordinate system from origin of 

fixed coordinate system to origin of moving 

coordinate system 

vector in fixed coordinate system from origin of 
fixed coordinate system to lower attachment 
point of actuator i 

compliance selection vector 

T k i ne tic ener g v 

LT3 Euler angle transformation matrix 
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torque 


u 


rp 


error in length due to the error in position 


u 


rf 


error in length due to the error in force 


veloc i ty 


vector of desired Cartesian position 


vector of position error in Cartesian variables 


vector of velocity error in Cartesian variables 


position of the react ion surf ace 


cx 


unknown parameter vector which is vector of root 
of f( ) 


w 


angular velocity 


w 


angu 1 ar acce 1 er at 1 on 


'±' 


a n y u 1 a r or is n tst ion o 


■ 1 atf or m 


A 


posi t i on compensat i on f unct i on 


viii 
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INTRODUCTION 


Robotics has many potential space applications that 
can be found in the space station research program C 1 3 , 
such as autonomous repair and replacement capability, 
automated maintenance, etc. One of the most essential 
robotic tasks is the assembly of large strucutures in 
space. Product assembly in space such as mating and 
fastening of parts requires a very high precision. There 
is however a tolerance problem that exists in assembly. 
The positional tolerance of many assembly tasks are quite 
high (thousands of inch), while those of a robot 
manipulators are only of the orders of inch. To solve 
this problem, compliance can be provided to the 
manipulator so that it can comply with the task-imposed 
constraint C23. Compliant motion control is concerned with 
the control of a robot in contact with its environment 
C33. Compliance can be provided passively by the 
deformation of a mechanical structure, such as mechanical 
spring or actively by a control system employing force 
sensors C4-53. 

In many applications, there is a need for simultaneous 
control of positon and force. There are two approaches to 
compliant motion control. Impedance or stiffness control 
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specifies dynamic relationships between force and position 
but do not specify desired forces or positions C33. Hybrid 
control that controls position/orientation along specified 
degrees of freedom and independently controls force/torque 
along the remaining degrees of freedom LSD. The hybrid 
control is more suitable for planning a manipulator task 
and in this report we will present the development of a 
hybrid controller C73 for a si x-degree-of-f reedom robot 
recently built at NASA/GSFC LSD to study robotic assembly 
in space. 

Since the dynamic equations of a si x-degree-of-f reedom 
robot is highly complicated, we start our design with a 
two-degree-of-f reedom model. The results of simulated 
system will be presented and discussed. 

2 . IHE _NASA_R0B01 

Recently a si x-degree-of— freedom robot was built at 
the GODDARD SPACE FLIGHT CENTER (GSFC) to study the 
automated assembly of NASA hardware L83 for potential 
applications in the space station C13. The robot consists 
mainly of a lower fixed platform, an upper movable 
platform and an intelligent end effector (1EE) (Fiq. 1A). 
The movable platform is supported above the fixed platform 
by six axial rods that are extensible by recirculating ball 
screws. Six stepper motors were used to derive the ball 
screws and consequently the movable platform to provide 
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the gross motion 

of the 

IEE. 

The 

IEE ie 

d t t c<uhed 

to 

another plat-form 

suspended 

from 

the 

movab 1 e 

p latform 

by 


six spring-loaded pistons that provide passive compliance 
to the IEE by permitting strain on two opposing springs 
acting in the piston (Fig. IB). Using force feedback;, the 
robot has been able to perform several assembly tasks such 
as inserting a peg into the hole, screwing a bolt into a 
threaded hole- However, since the compliance was provided 
passively, the assembly process was very slow, for example, 
the robot took: 3 minutes to complete the inserting of a 
peg into a hole. Therefore, research effort has been made 
to study the implementation of the hybrid control scheme 
into the robot system so that the IEE compliance can be 
provided actively by six electromechanical actuators 
replacing the existing spring- loaded pistons L103. 

3 - IHE_HYBR2P_CONTROL_ SCHEME 

In many applications, it is necessary to control 
simultaneously position and force. Such control is 
called hybrid control, that controls position /orientation 
along specified degrees of freedom and independently 
controls force/torque along the remaining degrees of 
freedom. The degrees of freedom are in a Cartesian 
coordinate system which is called the constraint frame and 
is denoted by the symbol CC3 Cl 1 — 133. In a hybrid 
controller scheme, each actuator control signal is 


composed 


of several 


components , 


one for each force 



controlled degree of freedom in CCD and one for each 
position controlled degree of freedom. The actuator 
signal is computed by C73: 

N 

T T C s fif aX,1> (1) 

1 j-l' ^ 3 ID 3 3 

Where 

Ti=torque applied by the ith actuator 

Af zrforce error in jth degree of freedom of CCD. 

AX . =posi t i on error in jth degree of freedom of CCD. 

P. and ^ .. = force and position compensat ion functions* 
13 ID 

respectively;, for the jth input and the jth output. 

S ^component of comil iance selection vector. 

3 

The compliance selection vector S, is a binary N-tuple 
that specifies which degrees in CCD are under force 
control ( s j j — 1 ) and which are under position control 

(s j j=0) . 

Fig. E illustrates the implementation of the hybrid 
control scheme into existing NASA robot. The complete 
robot system consists of a position sensor, a 

s i M-degr ee-of -f reedom force sensor , IE controllers with 6 
controllers for the position control loop and 6 
controllers for the force/ torque control loop, 6 linear 
actuators and the NASA robot. The compliance selection 
matrix is a (6x6) diagonal matrix. 

Two feedback loops compute errors in both applied 
force/torque and actual position, with the lower loop 


controlling the force/ torqe . The two independent feedback 
loops with 6 controllers? provide the* drive signals to the 
actuatorsC 103 . 

Current research does not prescribe particular control 
laws for the regulation of errors [73. Therefore? our 
research objective is to make a comparative evaluation of 
different types of control laws in order to select an 
optional set of controllers for a particular robotic 
assembly task. Doing this a guideline can be developed for 
the design of these controllers. The controllers and the 
coordinate transformations are then implemented on an 
appropriate computer . 


<+ . COORpiNAIE_TRANSFORMMION5 

There are two coordinate transformations ? one for 
transforming coordinate from Cartesian space into joint 
space? and one from joint to Cartesian space. With the 
arrangement of the linear actuators as seen on Fig. 1? the 
robot does not have independent drive systems for each 
degree of freedom. The precision motion is achieved in all 
degrees of freedom by a combination of actuator extension. 
Therefore? a transformation transforming desired position 
and orientation of the IEE into actuator extensions is 
needed. This problem is well-known as an inverse 
kinematic problem [153 that in general ? has non-unique 
solutions and is very difficult to solve because of 
non-linearity. However? since the NASA robot has a 


special configuration* its inverse kinematic problem has 
a unique closed-form solution. 

On the other hand, the forward kinematic problem of 
the NASA robot does not have a closed-form solution and is 
solved iteratively. As part of the hardware, six linear 
voltage differential transformers (LVDT) are mounted 

parallel to the six linear actuators to measure their 
extensions. Position feedback is then achieved by taking 
the actual extension available from measurements and 
transforming them into the position and orientation of the 
IEE. This problem is a forward kinematic problem of an 
open loop robot like the Stanford manipulator. 

Fig. 3 shows the general orientation of the 
manipulator with two coordinate systems, and Fig. 4 
illustrates the vector relationships between the origins of 
each coordinate system and the actuator attachment point 
of each platform. These relationships yield the vector 
equations 

r =A +R < 2a ) 

i i 

r =B +1 (2b) 

i i i 

Subtracting (2b) from (2a) and solving for 1 yields 

i 

1 =A +R-B (3) 

i i i 

where 1 is defined with respect to the fixed reference 

i 

frame • 

Since the known vectors A are defined with respect to 

i,m 


apply an 


the moving frame. we must apply an Euler angle 

transformation in order to qet the corresponding vector A^ 

in the fixed reference frame. Applying this transformation 

? we obtain 
T 


1 . = C T 3 A. +R-B. 

1 i,m l 


< A) 


In Equation (A) A. and B. are known constant vectors 

i,m 1 

T 

and R and CT3 are calculated from the given values of 
x ? y ? z ? e and $ . 

The inverse could be obtained if the vectors 1^ were 

available? but LVDT measures only the magnitude of the 

corresponding actuator length jl. j and not the 

a 

required vector. However? because of its closed-loop 
configuration? the forward kinematics problem of the NASA 
robot can be solved by solving six nonlinear equations 
with six unknowns (x?y? 2 ?i .0 and ®>. Newton-Raphson method 
is employed to solve this problem Cl&l. 

The iteration formula has the form 

-1 


r ?f(« n ) 



f<« > 

n 


( 5 ) 


Knowing the actual magnitude of the actuators » function 
f^(a) can be defined as 


f i <« ) =i i 


h 



(6a) 


*~7 



where 


and 


y 


e 

§ 


f^ < ex ) 
f 2 (a) 


f 6 (0<) 


(6b ) 


(6c ) 


and j 1 ^ J a is the actual magnitude of the actuator i 
obtained from measurement. Using an appropriate set of 
initial conditions? Equation (5) is repeated until desired 
accuracy is met. 

The Newton-Raphson ' s method has been applied in the 
NASA passive compliant robot system to update the position 
of the compliant platform relative to the compliant base 
and to compute the position of the movable platform with 
respect to the fixed platform C83. This technique 
requires ample memory and enormous computation time. 
Therefore? it is suggested to investigate other Newton- 
Raphson - ’ s method. Some of these methods are known as 
Quasi-Newton methods C17-183. The Newton-Raphson * s 
iteration diverges if the initial guess is not close to 
a correct solution C193. Using Newton-Raphson-Kantorovich 
theorem C183 it is possible to find how close the initial 
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guess should be . Consequent 1 y , to 
the Newton-Raphson iteration it 
the Newton-Raphson-Kantor ovi ch 
software l 10] - 


assure the convergence of 
is necessary to implement 
theorem using computer 


5- Xtlf- -DEG RE E~ OF -FREE DOM _R 0 BO X 

In order to examine the performance of proposed hybrid 
control scheme, we begin our study with the 
2— degree— of —freedom, robot since the dynamic behaviour of 
the complete s i x-degr ee-of-f r eedom robot is highly 
comp lex. 

Fig. 5 illustrates a simplified version of the 
physical robot consisting of 2 linear actuators;. 2 LVDT’s, 
2 force sensors and an IEE. The actuators are connected 
to a fixed platform using one— degree-of-f r eedom gimbals. 
The LVDT's are mounted parallel to the actuators to 
measure the lengths 1^ and 1^ . The 2 force sensors are 
mounted in series with actuators to measure the reaction 
forces in the direction of the actuator axes. 


5 - 1 KINEMATICS_OF_XHE_MANIPyLAIQR 

Manipulator k inemat ics is a study of the geometry of 
manipulator arm without regard to the forces which cause 
it. In t h i s section, the - rri e* t h s* m e* t i c a 1 ' e 1 e; t i o j t s . e g u i ' e d 
to describe the system motion and the fundamental equations 


that govern kinematic behavior are derived. 


5.1.1 POS I ION_ ANALYSIS 


The position of the point P at the tip of the hand 
with respect to the X-Y coordinate (Cartesian frame) is 
known if its horizontal and vertical projections x? and y 
are known C 10D - However? only the lengths 1^ and 1^ are 
known from the LVDT ? s . From the geometry of Fig. 6? we 
obtain 

2 2 2 

x +y -l^ (7) 

r 

222 

and C d — >: ) +y <S) 

where d is known from the geometry of the system. Since 1^ 

and 1 are variables from measurements? we can rewrite 
2 

Equat i ons ( 7 ) and ( S ) as 
222 

f ( x ? y ) = x +y — 1 —O ( 9 ) 

1 1 

2 2 2 

f (x,v)s(d-x) +y -I “0 (10) 

2 y 2 


To solve these equations for x and y? we can use iteration 


techniques 

such as Newton— Rap h son 

method 

wh i ch 

was 

desc^ i bed 

in section 4. For P— degree— o 

f -freedom 

case ? 

the 

squat i ons 

can be sol v ed b y anal 1 1 1 c a 1 

method . 

Simpl if ■; 

f i r Q 


Equation (8) as 


2 2 2 2 
x +y -i-d -Edx=l 

2 


( 11 ) 


2 2 

and substituting < >; +y ) from Equations (7) into (11), we 
obtain 



5 . 1 . a VELOC I IV derivation 

A manipulator can, in general, be described as a 
series of links connected at joints. The angles between 

the links, called the joint angles? are typical joint 
variables. However, some type of links (prismatic joints) 
can grow longer, and in that case the joint variable may 
actually be the length of the link [63. 

The position of all the links of a manipulator of n 
degrees of freedom can be specified with a set of n joint 
variables. This set of variables is often referred to as 
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the nx 1 joint vector- The space of all such joint 
vectors is referred to as joint space. There are three 
repr esenta t i ons of a manipulator’s position and 

orientation: descriptions in actuator space? joint space 
and Cartesian space [ 153 . 

In section 5-1.1? we derived the kinematic equations 
of the hand in Cartesian coordinate system. The forward 
kinematic transform for this manipulator relates the X-Y 
position of the hand to the joint variables. For our 
E-degree-of -freedom robot with two prismatic actuators? the 
joint variables can be chosen any of the following sets: 
(1 1? e ! ) , <1 2 , e 2 > » < 6 i> ®2> and < 1 1 , 1 2 ). As the LVDT’s 

can measure the lengths < ? 1 2 >* we chose these variables 

as the joint variables. The inverse kinematic relates 
the set of joint variables to the desired position and 
orientation of the hand relative to Cartesian coordinates 
(Equations (7) and (8) ). 

In order to move the end-effector in a specified 

direction at a specified speed? it is necessary to 

coordinate the motion of the individual joints. For this 

purpose we drive the differential relationships between 

the joint displacement and the hand location [33. The 

velocity relationship between the joints and hand is 

determined by the manipulator Jacobian 
* 

V=J1. (16a) 

Differentiating Equations (13) and (15) with respect to 
time 


IE 




Rewriting the Equations (17) and (19) yield 


• 

( 1 /dll^ 

-( l„/d>L 
2 2 

< SO ) 

and 

2 2 
l x (d +1 2 

2 

- 1 ! > 

2 2 2 

1 (d +1 -1 ) 

2 12 


• 

y= 

dQ 


*1 + ”1 ' *> 

(SI ) 

where 


2 2 

Q=t4d lj 


2 2 2 
-n +d ) 3 
2 

(SS> 
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Writing in vector form 


L 



« 

y 


fA 


1 






Thus the matrix 


( 23 ) 


J= 



A 1 
2 

A 

4 J 


is the manipulator Jacobian;. 
Where 


< 24 ) 


A =1 /d , A =-l /d f A =C1 (d 2 +l 2 -l 2 >3/dQ 

11 22 31 21 


and A = Cl <d 2 +l 2 -l 2 ) 3/dQ . 

4 2 12 

Note that the elements of the Jacobian are functions of 
joint displacements, and therefore vary with the arm 
conf igurat ion . 


5.1.3 ACCELERATION^DERIVATI ON 

The linear acceleration of point P with respect to 
Cartesian system can be obtained by differentiating 
Equations (20) and (21) as the following: 

- •• . 2 .2 

x=(l /d ) 1 - <1 /d > 1 +1 /d - i-., 

1 12 2 1 2/d 


<H5) 


o r 


•• •• •• 2 2 

<S6> 

2 22 222 22 
<B 1 i: i +E1 l >2 \ { 2 " el l ^ > 4B l d i \ " 2B 1 E 2 ( >1 \ ~h \ \ *2 > 

• • 

y= : 

3 

dQ dQ 

2 2 2 

X 1 B 1 <B 2 i 2 +21 l 3 2 i l i 2- £1 2 i 2 ) J 2 B 2 

• « •• 

+ ( ) 1 + + ( ) L 

dQ dQ dQ 

2 2 2 2 
^d B 2 i 1 J 1 i 2 ; 2 -eB 2 <i 1 ^i 2 i 2 -i 2 J‘> 

<27) 

3 

dQ 

wher e 

2 2 2 2 2 2 
E*i=d +i 2 - l x and & 2 =d 4^ -1 2 (£8) 


5 . 2 PYNAM^CS_OF_IHE_MANIPyLATQR 

The dynamic behavior can be described in terms of the 
rate of change of the arm configuration in relation to the 
joint torques exerted by the actuator 131 . Equations of 
motions*, govern the dynamic reponse of the arm linkage to 
input joint torque- There are two methods to obtain the 
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equations of motions Newton— Euler ? and Lagrangian 
f or mu 1 at ion . 

5 - a . 1 NEWION— EULER _FORMlJL AT I ON 

The equation of motion is derived by the direct 
dynamic system interpretation of Newton’s Second 
Law of Motions? which describes dynamic system in terms of 
force and momentum. The equations include the constraint 
forces acting between adjacent links which must be 
eliminated. The dynamic equations of a rigid body can 
also be represented by two equations; 

i)Newton ? s equation of motion for a mass particle? which is 
the translational motion of the center of mass 
(centroid). For a rigid body where center of mass is 
accelerating with v ? the force F? acting at the center of 
mass is 


F i =m i V Ci 


( 29 5 




where? m is the total mass 
velocity of the centroid of 
base coordinates (inertial 
inertial force C15J. 


of the body? ? the 

link i with respect 
ref erence frame ) ? and 


1 1 near 


to the 


m. V. , 

l Cl 


iiJEuler’s equation of motion? which is the rotational 
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motion of the centroid. For a rigid body rotating with 
angular velocity w, and angular acceleration w , the moment 
N? which must be acting on the body to cause this motion is 

N=I w +w>:I w (30) 

c c 

where I is the inertia moment of the body? I w inertia 
c c 

torque? and is the angular momentum. 


5.2.2 L AGR ANG I AN FORMUL AT I ON 


In the 


i r; r-\ rm 


l w cm vj 


Lagrangian 
in terms of 


method, the system dynamic behavior 
w o r k a n d e n e r g y ? using general i zed 


coordinates. All the workless forces and constraint 
forces are automatically eliminated in this method. The 
resultant equations are generally compact and provide a 
closed— form expression in terms of joint torques and joint 
displacements. Furthermore? the derivation is simpler and 
more systematic than in the Newton-Euler approach. As in 
kinematics, we need to solve the inverse problem of 
finding the necessary input torques to obtain a desired 
output motion (inverse dynamics). 

Let q * q be generalized coordinates? T and P 

1 n 

the total kinetic and potential energy stored in the 
dynamic system. We define the Lagrangian “L" by 


L'q ,q >=T-P 
i i 


(31 > 
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o r 


d/dt ( aL/aq )-3L/aq =Q i i = l ? r. (32) 

where £L is the generalized forces corresponding to the 
generalized coordinates q^. 

5.2.3 EQUAJ10NS_OF_MOJION 

To formulate the equations of motion of the robot? we 
employ the Lag rang i an and Newton-Euler approaches. 

i) Lagrangian method 

F r ig. 6 represents the forces acting on the robot 
system. The robot task is to keep in contact with a 
reaction surface while maintaining a constant applied 
force f Q . This task occurs for example in the process of 
painting a surface. The robot is moving in a horizontal 
plane <tc» the right) ? while maintaining a constant 
vertical force on the reaction table. For simplicity?, we 
assume that the table is smooth enough so that the table 
friction is negligible?, joint and actuator friction are 
a 1 sc negligible. Therefore? the reaction force will be 
normal to the hand <f y ). 

Using the Lagrangian formulation? we will derive the 
equations in terms of the two independent joint variables? 
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the length 1^ and the angle e_^ [£13. 

We begin by computing the velocity of the centroids 
and the end-effetor P . From geometry, the 
Cartesian positions are 


=1 cose 

PI 1 


y =--1 sine 

y P 1 1 


« = ( 1 / £ ) c o s 

Cl 1 




and y. =-(l /£)sine 
Cl 1 1 

S i m i 1 ar 1 y , 


=d- ( 1 /£)cos e_ 

on n o 

A. 


and y =- ( 1 / £ ) s i n e 
C2 2 2 


(33) 

(34) 
(35a) 
(35b ) 


(36a) 


(36b ) 


To eliminate e» and 1 from the equations, we can write 
2 2 


\ COZ e i = d-1 2 CDS0 2 

(37) 

and -l^sin e^- - ^ sin 

(38) 

Substituting from Eqs. (37) and (38) 

into <36a) and (36b) 

we get 


>: C 2 = d - ( d - 1 1 c o s e^ > / £= ( d + 1 c o s e ^ ) / £ 

(39) 

and V C2 =-1 1 sin e i /2 

( 40 ) 


I 

Differentiating the above equations with respect to time 
J yields the Cartesian velocities as 


19 



i 1 coE 6 i~ i i sin vi i [ cos9 i e ii r i i 1 


yj [-I 1 sine 1 -1 1 CC.B9 1 ; 1 J [sin^ -l^o^ J 


< 1 x cose^^sin e^/ 2 ' 

(43) 

-<i 1 sine i + i i cos e x e 1 > /£ _ 


The magnitude of the velocity vectors can now be computed 


2 2 ’ 2 " 2 
V = 'v = <>; ) +( v ) 

Cl C2 1 * Y 1 


(43a) 


—Vq 2 =1 / '+ c ( ^ CDS ^ +( 1 1 £in e x - 21 ! ^ sin ^cosej^ e x 3 + 


" 2 . 2 • 

1 /4t < ^ sin e^) +<l 1 cose 1 +5^ ^ sine 1 cose 1 3 

( 43b ) 


2 2 2 


'4- v a-”i +1 i J i > /4 ’ 


(43c ) 


Similarly? 


2 .2 2 2 

V p “ 1 1 + 1 1 ®, 


(44) 


The kinetic energy of a mass m with linear velocity V and 


mgular velocity w is 

T=l/E mV 2 +l/E Iw 2 


{ 4Da ; 


Note that as we assumed point mass, then the moment of 


inertia I^=I 2 =0 , thus Iw =0. 


eo 



Th eref o r e - 


2 


T =2 ( 1 /£ m V ) 
i l 


i — 1 ! • « • • 5 N 

(46a) 

T=l/2 M V 1 2 +1/2 
1 Cl 

M V 2 
2 C2 

+ 1/2M V 2 
3 P 

(46b ) 

where M < M and M 

12 3 

are 

the masses 

of the two 1 inear 

actuators and the 

IEE , r 

espectively . 

Substituting the 


values of the velocities into (46b) yields 

.2 2.2 

T = [ ( M +M )/8+M /£] ( 1 +1 e ) (46c) 

12 3 111 

The actuators are assumed to have identical masses? 

M^=M 2 =M . This assumption changes (46c) to 

2 2 2 

T= ( li/4+M /£ ) ( 1 + 1 e ). ( 46d ) 

3 111 

Now ? we find the potential energy? using the formula 
P=mgh (47a) 

where h is the height and g the acceleration of gravity. 
Thus ? 

=Mg(— l^sine^/2) (47b) 

P =Mq(-l sine /£) <4S) 

2'2 2 

Rj =M^q (-1 1 sin6 i ) (49) 

where from Eq . (38) 

1, sin 6 /2=1 sin 6 V2 - (49a) 

112 2 

£1 


The total potential energy is 


F'^Py +F’ 2 +P 3 =- ( h+M 3 ) g 1 1 s i n e 3 < 50 ) 

We now combine Equations (46d) and (50) to produce the 
Lagrengian for this manipu 1 ator 

.2 2.2 

L=T-P=<M/4-+M 3 /2) ( l y +\ e 1 ) + (M+M 3 >gl 1 sine! (51) 

The free-body diagram of the Link 1 is shown in Fig. 7. 
The total force applied on the system is 


F=t 


ml 


- f m2 


cos e-i 


■ySir.0! 


(52) 


where 


0=tt— ( + ©2 ) 


(53) 


Theref ore , 


F ^ f ml +f n,2 


C O S ( 


V 


e 2 )—f y si r,e i 


(54) 


Although there is not any torque about 0^ , but when the 
second link is extending or contracting, it makes the first 

one to rotate. So we can state that the forces f m2 and 
f y apply a torque about 0! as the following: 

T e =f m2 h sinfl +'fy 1 iCOS 0i (55) 

or 

" r 0 =I -tm2 sin(0i +®2 )+F y CD5 e l-* ^ (56) 


22 



In Equations (55) and (56) f is pushing and f is 

ml m2 

pulling. Using Equation ( 3E ) we car, find the forces 
applied by the linear actuators by considering 


3 L 

= ( M/E+Mn ) 1_ 

• 3 1 

3l i 

d 3 L 

— = ( M/S+li ) 1 

dt 3i 3 1 

1 


3t- 


al 


— = (M/£+M, )1 e + ( M+M, ) qsin e . 
, 3 1 1 3 " 1 


(57) 


(58) 


(59) 


Then ef ore , 


F= ( M/S+M 3 ) 1 1 -(M/E+M 3 ) l ± e, 1 -(M+M 3 )gsir, 6l 


( 60 ) 


Similarly, from the torque 


3 E 


2 . 


= ( M/E+li ) 1 e 

3fl 1 311 

d 9 L • 2 ** 

= ( M/E+M > (El 1 e +1 e ) 

dt M 3 11111 




3 e. 


= <M+M >gl cos e 
3 1 l 


(61 ) 


( 6 £) 


(63) 


Thus , 


T = ( M/S+li ) ( El 1 e +1 6 ) - (M+M ) g 1 cose 

0 3 11111 31 1 


<6h) 


Substituting the values of I- and 1 from (54) and (56) 
into Equations (60) and (64) yield 


S3 


f n,l +f n,2 < V e 2>- f y si " 6 1= (M/2+,1 3 > h -‘ H/2+M 3 > Vl 


< M+M-j ) geinO^ 


( 65 ) 


f m2 sin<e i + e 2 )+f y cDS0 1 = (M/2+M 3 ) + <M/2+M 3 ) ( 2^ 

( M+M j ) gcc<s0^ 


e, >■ 


( 66 ) 


To write the above equations in terms of joint variables 1^ 
and 6^ , we must eliminiate 62* From trigonometry we obtain 

cos( e ^+ 6 2> =cos e^cos ©j-sin 6 ^sin02 (67) 


Substituting the values of cos02 and sin02 from Equations 
(37) and ( 3B ) into (67) we get 

tobi ®2 + ®2^ —cos 0 ^l ( d — i^ cos 6^ ) / ’1 2 J — sine^ l ( li si n0^ ) /' ’ I2 3 (68) 

or cos ( 0^ + ©2) - ( dcos 6^~ 1^ )/l 2 . (69) 

S i m i 1 ar 1 y > 

sin( 0-^ + 0 2 ) =sin e-jCose 2 +c.ose^ sine 2 (70) 


or 

sin( e x + 0 2 )=sin0^ [(d-l^ cosOj^ ) / 1 2 3+cos 0^ C ( 1^ sine^ >/l 2 3 
=d sine-,/12 (71) 

From Fig. 6? we obtain 

2 2 2 

1 2 =d + 1^ -Ed 1^ cos 0^ (73) 


2 2 

I2 = ( li +d -Edl^cos0^ 


'4 


( 73 ) 


Substituting i 2 into Equations (69) and (71) 


dcose^ -1 


cds ( e + e > =- 
l 2 


2 2 

( 1 +d -Sd 1 cose ) 
1 11 


d sine 


and sin ( e + e > =- 
1 2 


2 2 '4 

( 1 +d -Sd 1 cose ) 

1 11 


(74) 


(75) 


Equations (65) and (66) can be rewritten as 



+f a( t )-f sine 
m2 y 

b(t)+f cose =K 
y 11 


1 K 'l l l k l 1 1°1 


e 1 +SK 


l 1 ! 6 ! 



-K 2 gs i n e 
gcose^ 


(76) 

(77) 


where 

K 1 =M/S+M 3 ; K 2 =M+M 3 ( 78 ) 

a ( t ) = ( decs -1^ ) / c ( t ) ;b(t)=dsine/c(t); 

2 2 ‘4 

c(t) = (l 3 +d — Sd 1^ cds ) . (79) 


and 

f ml 

and f 
m2 

are 

actuator 

forces . 

The force f 

y 

is 

the 

reaction 

force 

produced 

through 

contact with 


the reaction surface and is modelled as follows: 



when in contact 


(y-y ) <0 
y y E 


otherwise 


(y-y 


E 


) >0 


where y and K denote the position and stiffness of the 
E £ 

reaction surface? respectively C33. The consent K is 

£ 


S5 


6 

typically of the order of 10 N/m for a stiff environment. 

The term EK^l^O^in Eq . (7~) represents the Coriolis 

force acting on Link 8 due to the Coriolis effect (when a 
mass particle m moves at a velocity of V relative to a 
moving coordinate frame rotating at an angular velocity 
w, the mass particle has the so-called Coriolis force given 
by Em(w::V>>. The last term k^gsine^ and k^ geos 6^ in 
Equations (76) and (77) accounts for the effect of 
gravity. The second term on the right side of Eq . (76) 

which is proportional to the square of the joint 
velocities shows the centrifugal force. 

ii) Newton— Euler Method 

In order to reduce the computation t i me for the 
simulation of the robot system, the equations of motion in 
terms of the Cartesian variables X and Y are needed. 
Fig. 8 illustrates the free body diagram of each link of 
the robot system. Applying Newton formulation for IEE, 
we obtain 

Fix +F 2 x <81 > 

-Fly +F 2 y -M 3 g+fy=li 3 y (88) 


Knowing that V c ^=V C 2 =Vp/E ? for Links 1 and 8 we can 
wr i te 


86 


f COS A -F =M x/d 

ml 1 lx 


( 83 ) 


-f sing +F "Mg=My/S 

ml 1 ly 


( 8<4 ) 


f cose -F =M;:/2 

m2 2 2x 


(85) 


f sine -F -Mq=My/E 
m2 2 2y 


( 86 ) 


Substituting the values of F and F from Equations (83) 

lx 2x 

and (85) into (81) and F , F from (8^) and (86) into 

ly 2y 

(82 ) , and using the relations of (35) and (36) » we obtain 


f ( >: / 1 )+f C ( d— ) / 1 D=K 

ml 1 m2 2 2 


(87) 


f (y/1 )-f (y/1 )+f =K y+K g 

ml 1 m2 2 y 2 3 


{ OO \ 


where 


K =2M+M 
3 3 


< 8 ? ) 


and 1^ 1^ ? x and y are related through Equations (7) 

and ( 8 ) . 


5 . 3 HYBRID_C0NTR0L_0F_IHE_2-DEGREE-0F“FREED0M_R0BQI 

The block diagram of the hybrid control system is 

shown in Fig. 9. Both position and velocity feedback are 

employed to improve the response. we assume that the robot 

is to move from the initial position x =A , v =B to the 

i ' i 

final position x =x ? while exert inq a force f =f in 

f d * dy 0 


£7 


tut vertical direction. Manipulator is moving in a 
horizontal plane bo that gravity may be ignored C 6 3 . The 


desired velocity in X direction will 
direction is zero. 


be x =Q while in y 

d 1 


5 . 3.1 pqsiiion_and_velociiy_conirol 

For a S-degr ee-of-f r eedcm robot the selection matrix 
defined in Eq . ( 1 > ? will be 



Since we are interested in controlling position in x 


direction (s =0)> and force in Y direction (s =1), the 
11 "0 0 " 22 
corresponding compliance matrix is S= 


0 1 


and CI3-CS3=| 


fl 01 [0 01 fi 0 


0 10 1 


J L_ J L 


where 111 is the identity matrix. The position error is 


"x 1 rx < t n r ai rx ( t > i m-x < t > • 

AX(t)=X - A(q(t))= d - = = (91) 

d y y< t) B y(t ) B-y<t> 

- d J b jljl jl j 

This error is mapped into the position controlled 
subspace. The compliance matrix S serves as a mask to 
separate the force controlled and position controlled 


degrees 

obtained 



of 


as 



freedom. The Cartesian error signals are 


01 [A-x( t>“| TA-xCtr 

= *C C I 3 “ [ S 3 } A X < t > = - (93) 

0 0 B-y ( t ) 0 


E8 


Transforming them back into the joint space, we find the 

corresponding joint variables errors. Since X is a 

e 

differential change, we can use the Jacobian to find the 
corresponding (approximate) differential change in the 
vector of joint variables 


q ( t ) = 

Recal 1 ing 
f o 1 1 ows : 


1 <t> 

1. < t ) 
2 


J from 


Equat ion 


where 


-1 Ad j [ J 3 

J _ 

Det | JJ 



2 2 
<d +1 



) 


Ad j | JJ= 


dQ 


2 2 
(d +1 2 



) 


dQ 


(£4) we find its inverse as 


(93a) 



(93b) 


and 

2 2 2 


2 2 2 


[V d + i x -i 2 >‘ 

*2 

1 l (d +1 2 "'I >' 

i *t ( — 




Uc l 1 J | — — — — 

d 

dQ 

T — ~ — 

d 

dQ 


(93c ) 


iii 2 < d +i 2 -ij 


Det 


i j r 


2 

d Q 


( 9 3d ) 


or 


Det 


i j r 


Q 


(93e ) 


£9 



Theref ore , 


-1 

J = 


h ~ 1 2 +d 


El 1 d 
2 

<1 2 -lj. 


2 2 
+d ) 


2 2 2 2 
C4d l x — ( 1 -l -1 2 


' 2 »/ 2 
) 3 1 


21 x d 

2 2 2 2 2 


2 1/2 


L4d l x -( l x -1 2 + d ) 3 


Sl 2 d 


Sl 2 d 


For simplicity, we define 


-1 
J = 


L E 3 


: 4j 


where * s are given in Equation <9V>. Thus 


q ( t ) = 
e 


i e i<t>- 


i e2 (t) 


-i 

=j <t) 


q ( t ) = 

El 

r~ 

rsi 

LU 

’A-x(tr 


E x C A— x ( t ) 3' 

e 

e 3 

E 4 J 

(■) 

L 

. 

E 3 C A— h ( t ) 3 


The error in joint variables will be 


1 ( t)=Ei CA-x(t>3 

el 


1 ( t )=E 3 CA-x (t) 3 

e2 


Applying the same method for velocity control, we 


AX f t )=X - A(q ( t ) > = 

d 


• 

“ w - 

1 

"x(t )“ 

r 

' d 

_ 


sr 

• 

y 


y ( t ) 


L d- 



L 


Q — ( t ) 

1 

0 -y < t ) 


1 

<tr 


'1 

0* 

r 

X (t)=l 

e 

= 




e 

y (t> 


o 

0 



e J 



- 

L 


Q — >; < t ) 

1 

0 — y < t ) 


Q ( t ) 

1 

0 


( 94 ) 

(95) 

(96) 

(97a) 

(97b) 
(97c ) 
f ind 

( 98 ) 

(99) 
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( 100a ) 


• 

ri 

( t )‘ 

q ( t ) = 

el 

• 


e 

1 

( t ) 


e2 


• 

'El 

E; 

q (t ) = 



e 

e 3 

E 

Theref ore 

, the 

• 


• 

1 ( t ) = 

=E 

CO 

el 

1 

1 

• 


• 

1 ( t ) = 

=E 

CO -x 

e2 

3 

1 


_ 1 • 

J X ( t ) 
e 

t! 1 -x* ( t ) 
0 


e-l ( t ) i 

E 3 COj-xU)] 


< 100b ) 


>3 (100c) 

l 3 (lOOd) 

To move the joints and reduce the error, we 
(proportional derivative) controller for position- 


use PD 
Thus , 


u =q <t)CK 3 (101a) 

p e rp 

V =q < t > £ K 3 . ( 1 0 1 b > 

p e rpd 


The error in lengths due to the error in position and 


velocity are 


u <t)=u i t ) +v (t) (lOSa) 

rp p p 


For 

Link 1 we get 




u 

( t ) = 1 ( t ) K 

• 

+ 1 

( t ) K 

( 102b ) 

rpl 

el rpl 

el 

rpdl 


and 

for Link 2 




u 

< t ) = 1 < t > K 

• 

+ 1 

( t ) K 

< 102c ) 


rp2 e2 rp2 e2 rpd2 

• • 

Substituting the values of 1 ,1 . 1 - and 1 

el e2 el e2 

into (102b) and (102c), we obtain 


u (t)=E (A-x(t))K 


+E 

< Q - 

x ( t ) ) K 

( 103a ) 

rpl 1 

rpl 

1 

1 

rpdl 


u <t)=E ( A— >: ( t ) ) k 


h-E 

<Q - 

)< < t ) ) K 

( 103b ) 

rp2 3 

r P 2 

3 

1 

rpd 2 
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5.3.2 FORCE CONTROL 


Knowing the desired force 


■ f dx‘ 


N 

- f dy. 


1 

4-° 
1 


F a = 


and the actual force F ( t ) = 


as 


AF (t) =F d ( t)-F<t > = 


L f y J 


"0 


-f x (t)- 


— l 

1 

r 

f 


- 


= 


. f 0. 


. f y (t) . 


/ 0 - f y (t >. 


n the force error is computed 


(10^) 


Note that the coordinate system is parallel to the 
constraint frame. So, the force transformation matrix is 

r* 

± u 

o 1 

The Cartesian error signal is computed by 


F_ ( t ) = 
e 


xe 


L f ye 


=CS3AF(t) 


( 1 05a ) 


F e <t>- 


“0 

0" 


r 

*-“N 

-P 

1 

l 


0 

0 

1 


( 

1 


_f Q -fy (t)_ 


( 1 05b ) 


The corresponding Joint space error is found by 


x e <t)=J F e <t ) 


( 1 06 > 


where T e *t) = 


^rel 


f. 


re 2 


and J is the Jacobian transpose that i< 


derived as 


3E 



2 2 2 
V d +1 2 "'I >' 


2 2 2 

V d -1, -1 2 > 


( 107 ) 


T fG 


( 1 08 ) 


then 


pi G 2 ]r ° irwv 


E 4jL f 0- f yJ LWV 


( 1 09a ) 


f r ei(t)=G 2 (f 0 -fy) 


( 1 09b ) 


W t)= G 4 ( f 0 -fy > 


( 109c ) 


For force control » we will use PE (proportional error) 


error in force will be 


'rf rf 're 


(110a) 


n rfl ' t ) -K rf 1 f rel ( t ) K rfl G 2 ( f 0 f y ( t ) ) 


( 1 1 Ob ) 


u rf2 < t ) =K rf2 f re2 ( t ) «K rf 2 G 4 < f Q -f y ( t ) ) (11 Oc ) 


Finally the inputs to the linear actuators are computed by 
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e <t)=u +u =E <A— >:<t))K +E (Q —x(t))K + 

al rpl rfl 1 rpl 1 1 rpdl 


G <f -f <t> )K (111) 

2 0 y rfl 

e <t)=u +u =E < A— x ( t ) ) K +E <0 -«(t>>K + 

a2 rp2 rf2 3 rp2 3 1 rpd2 


Ga ( f -f ( t ) )K (HE) 

0 y rf 2 


5.3.3 LINEAR ACTUATOR DYNAMICS 


Since no transient analysis of DC linear motor 


is available? we assume that its dynamics is 


identical to that of DC motor. By neglecting the armature 


inductance and the load torque? the transfer function 


between the motor actuating force and the input voltage e 

a 

is obtained by CE71 


F (S) KK (J S+B ) 
m i m m 

= ( i is) 

E (S) R (J S+B >+K K 

a a m m i b 


where we assumed that F (S)=K T (S) and 

m m 

R =ar mature resistance 
a 

J =r o tor inertia of motor 
m 

K =back emf constant 
b 

K =torque constant 
i 

B =viscous frictional coefficient 
m 
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6 . 


SIMULATION 


In order to examine the behavior of the proposed 

hybrid controller? we conducted several simulation 

runs of the system. The robot dynamics and the hybrid 

control scheme are simulated using the System Simulation 

2 

Language (SYSL) developed by E Consulting for the IBM PC . 

6 . 1 BiSULJS_AND_piSCySSigN 

Fiq. 10 shows the responses of the system that weis 

controlled to stay at a fixed position X =10 cm? Y =-10 cm 

while mainting a constant force F =90 N. The responses had 

email overshoots in vertical force and position? which is 

caused by the initial forces of the motors <f„ and f -=6N > . 

ml m2 

The settling time was about 0.1 sec. The error in the 

-4 

horizontal position was about 10 cm and was not 
recorded in the graph. 

Figures 11 and IE depict the responses of the robot 
controlled to move from X =0 to X =1 cm horizontally while- 
keeping a constant force F =90 N on the reaction 

surface. The response shows that the robot reached the 
destination in 0.5 sec in an "overdamped " fashion. The 
average velocity in Fiq. 11 is & cm/sec and in Fig. IE 
7cm/ sec . The first case shows higher overshoot but faster 
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response. The vertical force and position were settled 
after about 0.1 sec with relatively small overshoots. 

Figures 13 and 14 demonstrate the control of 
force in the presence of position disturbances . The 
controller attempts to maintain a constant 90 N 
force at a fixed horizontal position on the reaction 
table while the table is moving away from the robot in 
y direction at a constant rate 0.1 cm/sec <Fig. 13), 
and 0.08 cm/sec. (Fig. 14). The quality of the force was 
quite good while there was some disturbance in the 
horizontal position (about 0.01 cm error ). When the 
surface stops moving (after 1 sec), control returns to a 
stable steady state. At the end of the ramp where there 
are acceleration there was a little disturbance in the 
force. In these figures, we note that the vertical 
position follows the movement of the table closely. 

In all simulation runs, the controller gains, actuator 
parameters and robot masses are adjusted until a stable 
response with little overshoots is achieved. Controller 
feedback gains and the numerical values of parameters used 
in simulation are given below 


K =39. N/m 

rpl 


K =35. N/m 

rp2 


K =K =25 . N/m 

rpdl rpd2 


K =K =0.3 
rfl rf2 


M=3 . 


Kg 


M =3.5 kq 

3 
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N/m 


d=E3 


cm 


5 

K =9;; 10 

E 

1 =1 =15 cm 

1 2 

7. CONCLUSIONS 

In this report the hybrid control was proposed to 
control the compliance of the existing NASA robot. 

A study of coordinate transformat ion of the I EE showed that 
its forward kinematic problem does not have a closed- 
form solution and consists of non-linear equations which 
Newt on-Raphson ? s method was employed to solve it. 

The kinematics, dynamics and hybrid controller of the 
2— degree— of-f reedom robot were presented and its 

simulation results were discussed. 

Even though we obtained some good results, no clear 
relationship between the controller gains and system 
responses have been obtained. Further research can focus 
on the development of a guideline to select controller 
gains for stable and responsive robot systems. 
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Fig. 2; The Hybrid Control Scheme 
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Fig. 4: Vector Relationships for Actuator i 
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Fig . 6 : Forces Acting on the Robot 
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NOTE: 
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Fig. 9: Implementation of the Hybrid Control Scheme for the 2-Degree-of -Freedom Robot. 
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Fig, ii: 


Kosponsos for Control of Constant Fojlcg wiiilo Moving 
on Horizontal Plano (V= 8. cm/soc ) 
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Fig. 13: Responses for Control of Constant Force on a Moving 

Table (V . =0.1 cm/sec ) 
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